Chinaxn v 合 作 期 乔 


第 39 卷 第 10 其 计算 机 应 用 研究 ol 39 No. 10 
录用 定稿 Application Research of Computers Accepted Paper 


面向 应 用 的 RGB-D 机 器 人 道路 坡度 融合 估计 方法 
凌晨 飞 ， 党 淑 雯 ， 陈 丽 


(上 海 工程 技术 大 学 航空 运输 学 院 ， 上 海 201600) 


摘 要 : 为 提高 机 器 人 在 移动 路 径 中 对 道路 坡度 的 估计 精度 ， 提 出 一 种 面向 应 用 的 RGB-D(Red Green Blue-Depth) 机 
器 人 融合 型 道路 坡度 估计 方法 。 首 先 ， 引入 随机 采样 一 致 性 生 法 守成 点 去 处理: 其 次 ， 采 用 改进 型 平面 拟 合 方法 完 
成 法 向 量 估计 ; 最 后 ， 采 用 余弦 聚 类 及 累加 平均 方法 实现 高 精度 道路 坡度 估计 。 实 验 结果 表 明 ， 该 算法 在 数据 集 下 
相 较 最 小 二 乘法 与 稀疏 子 空间 法 , 估计 误差 分 别 降 低 1.21%、2.13%, 在 实际 环境 下 较 最 小 二 乘法 平均 误差 降低 1.43”， 
这 证 明了 所 提 方 法 的 可 行 性 和 准确 性 
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Fusion slope estimation algorithm for RGB-D moving robot 


Ling Chenfei, Dang Shuweni, Chen Li 
(School of Air Transport, Shanghai University of Engineering Science, Shanghai 201600, China) 


Abstract: In order to improve the estimation accuracy of the road slope during the movement of the robot, this paper proposed 
a fusion slope estimation algorithm for RGB-D(red green blue-depth) moving robot. Firstly, the method used random sampling 
consistency algorithm to complete the point cloud processing. Secondly, the normal vector estimation followed an improve 
plane fitting method. Finally, the cosine clustering and cumulative average method were used to accurately complete the road 
slope estimation. Experimental results showed that compared with the least squares method and the sparse subspace method 
under the data set, the estimation error of the algorithm is reduced by 1.21% and 2.13% respectively, in the actual environment, 
the average error is reduced by 1.43° compared with the least squares method, which verifies the feasibility and effectiveness 
of the proposed algorithm. 
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0 引言 视觉 相机 主要 应 用 于 工件 缺陷 检测 09、 物 体 分 拣 n7、 视 觉 定 
03 等 ， 但 应 用 于 移动 机 器 人 的 坡度 估计 方案 较 少 。 从 实际 

移动 机 器 人 在 复杂 地 形 环境 下 、 尤 其 是 坡 面 道路 条 件 下 应 用 来 说 ， 相 较 激光 传感器 ， 基 于 视觉 传感器 的 移动 机 器 人 
运动 过 程 中 ， 准 确 并 稳定 的 完成 道 面 坡度 估计 成 为 关键 性 问 ”在 坡度 道 面 环境 下 的 应 用 更 为 合适 ， 但 目前 面向 视觉 移动 机 
题 ， 目 前 基于 激光 0 和 视觉 B4 传 感 器 的 环境 信息 探测 是 移 ”器 人 的 道 面 坡度 估计 方法 普遍 存在 仅 停留 在 仿真 无 法 面向 实 
动机 器 人 完成 地 图 构建 回 、 路 径 规划 外 的 主要 手段 。 激 光 传 感 ” 际 应 用 ， 估 计 误 差 较 大 的 问题 
器 因 其 测量 精度 高 这 一 优势 常 被 应 用 于 智能 交通 、 智 能 防疫 、 本 文 提 出 一 种 面向 实际 应 用 的 视觉 RGB-D 机 器 人 道路 
甚至 外 星 探测 等 领域 ， 而 提高 道路 坡度 估计 精度 及 稳定 性 成 ”坡度 融合 型 估计 方法 。 该 方法 系统 构建 了 包含 点 云 滤波 、 平 
为 吸 待 解决 的 问题 。 文 献 [7~10] 采 用 传统 最 小 二 乘法 直接 拟 下 分 割 、 法 向 量 提 取 、 余 弦 聚 类 和 累加 平均 滤 ; 
合 区 域 化 地 形 块 的 均值 平面 ， 拟 合 出 平面 的 倾角 即 为 该 区 域 合 型 算法 流程 ， 并 就 数据 集 和 真实 环境 下 进行 了 道 面 坡度 
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的 坡度 ， 该 方法 计算 简便 ， 设 计 精 简 ， 但 易 受 异常 值 影响 ， 占 计 实 验 。 经 与 传统 方法 比较 分 析 ， 结 果 表 明 所 提出 方法 在 
估计 误差 较 大 ;， 李海波 等 人 1 对 传统 最 小 二 乘法 进行 优化 ， 降低 估计 误差 、 提 高 坡度 估计 精度 具备 显著 优势 。 
采用 稀疏 系数 划分 子 空间 ， 再 对 子 空 间 进行 平面 拟 合 ， 最 后 1 ”算法 流程 

IL 
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在 子 空间 采用 随机 搜索 获取 最 优 平 面 ， 进 而 完成 坡度 估计 ， 

该 方法 可 有 效 减 少 异常 值 对 坡度 估计 的 影响 ， 但 需要 根据 平 本 文 算法 流程 如 图 1 所 示 ， 其 中 : 

上 硬 情况 预先 设 定 搜索 阐 值 ， 同 时 最 优 平面 的 随机 搜索 可 能 导 a) 对 得 到 的 点 云 信 息 ， 首 先 采 用 统计 滤波 方法 ， 滤 除 视 
致 计算 结果 陷入 局 部 最 优 ， 导致 坡度 估计 结果 有 一 定 随 机 性 ; 。” 觉 传感器 采集 所 得 点 云 表 面 的 离 群 点 ， 之 后 采用 随机 采样 一 


白 莎 莎 等 人 [9 在 中 线 拟 合 部 分 使 用 自 适 应 闵 值 拟 合算 法 , 通 。 ”和 致 性 算法 进行 平面 分 割 ， 分 别提 取 和 斜面 和 水 平面 。 为 提高 点 


过 点 云 数量 信息 和 坐标 信息 自动 计算 闵 值 ， 丰 富 了 滑雪 场 雪 云 处 理 效率 ， 设 定 平面 点 云 的 数据 判断 间 值 为 10000， 当 数 

道 坡度 计算 的 方法 。 据点 超 阔 值 时 ， 采 用 直通 滤波 方法 降低 数据 点 数目 ， 并 引入 
以 上 坡度 估计 方法 采用 激光 雷达 方案 ， 激 光 测 量 发 展 迅 ”随机 最 小 二 乘 方法 拟 合 点 云 平 面 提取 法 向 量 。 

速 、 测 量 范围 广 0329， 但 与 视觉 相机 相 比 ， 激 光 雷 达 所 获取 b) 对 提取 得 到 的 法 向 量 ， 首 先 采用 余弦 聚 类 ， 对 异常 向 

的 三 维 环境 信息 较 稀 玻 ， 丢 失 了 很 多 三 维特 征 ; 且 多 数 视 觉 ，” 量 进行 聚 类 剔除 ， 最 后 通过 累加 拟 合 方法 以 得 到 更 为 精确 的 


相机 的 体积 小 、 重 量 轻 , 对 机 器 人 的 承重 影响 较 小 05。 目 前 ， 坡度 。 
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点 云 信息 。 | 和 > 泪 波 处 理 。， >。 平面 分 吊 。 和 >。 庆 断 su 
上 


| 坡 道 角 度 。 | 人 累加 拟 合 ”< 一 ”余弦 聚 类 。 4 一 法 向 量 提取 


图 1 算法 流程 
Fig.1 Algorithm flowchart 


2 三维 点 云 拟 合 平面 的 法 向 量 获取 


2.1 滤波 处 理 

使 用 视觉 相机 进行 点 云 获 取 时 ， 相 机 深度 数据 易 受 外 界 
光线 的 干扰 ， 故 采集 到 的 图 像 存在 异常 噪声 ， 因 此 首先 引入 
统计 滤波 方法 对 点 云 内 每 一 个 点 的 邻 域 进行 统计 分 析 ， 估 计 
它 到 所 有 临近 点 的 平均 距离 。 假 设 某 个 点 云 为 
D={p. eR’},,,,, (1) 
其 中 : 1 表示 样本 中 点 云 中 点 的 总 个 数 ;， pi 表示 点 云 D 中 无 
序 点 ， 只 取 每 个 无 序 点 的 站 六 7 三 维 坐 标 。 计 算 点 bp 的 距离 


阔 值 do : 


本 


dawg = 一 十 QXxdsa (2) 
问 


中 : w 为 点 p, 的 邻近 点 个 数 ， domw 为 点 p. 与 其 邻近 点 pj 的 
氏 距 离 ， 其 计算 公式 为 4 =|p.-P 小 ; 4 为 阔 值 系数 ， dw 
点 Pp. 与 其 邻近 的 m 个 点 之 间 的 距离 标准 差 。 若 某 点 与 点 
E 离 大 于 ds。， 则 判定 为 离 群 点 ,将 其 剔除 出 点 集 2229。 经 
实验 对 比分 析 , 统计 滤波 的 参数 设置 为 m=20,g=2 时 , 可 
有 效 的 滤 除 离 群 点 P11。 
平面 分 割 
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2.3 法 向 量 提 取 
对 于 分 割 得 到 的 两 平面 ， 需 要 先 提取 其 平面 法 向 量 ， 再 
进行 夹 角 估计 ， 点 云 法 向 量 的 提取 过 程 分 为 法 向 量 估计 及 法 
向 量 定向 。 
2.3.1 法 向 量 估计 
根据 随机 选取 的 邻 域内 点 p; ， 并 设置 拟 合 邻 域 值 x， 可 


得 到 一 个 拟 合 平面 ， 平 面 的 法 线 即 为 该 点 的 法 向 量 ， 点 的 邻 
域 为 
N={p; (N53) i=1,2,...,7} (7) 
点 p; 的 三 维 空间 坐标 为 (%,y,4)， 可 得 拟 合 平面 N 的 一 
般 形式 是 
Fxt+Gy+Hz=1 (8) 
其 中 F,G, 有 ,7 为 所 得 拟 合 平面 的 平面 方程 参数 
Fi+G+H?=1 (9) 
使 用 随机 最 小 二 乘 进行 拟 合 ， 即 求解 以 下 最 小 值 
miny (Fx +Gy + He +1) (10) 
可 得 以 下 线性 方程 组 
F X2-F2 Wy-Xxy 下 -无 x 二 |「 严 
中 | Xy- TX yy-y? SE (11) 
H XZ- TXT yz-yxz ZZ? H 


Mi; 为 3x3 的 协 方差 矩阵 ， 对 其 进行 协 方差 分 析 
7 Pi -pi i Pi -pi 
M; = (12) 
Pix ~ Pix Pix ~ Pix 


M, xv = xv) ,i={1,2,3} (13) 


段 定形 成 道 面 夹 角 的 两 个 平面 中 ， 墙 面 为 斜面 ， 地 面 为 
水 平面 .引入 随机 采样 一 致 算法 以 完成 两 个 平台 的 点 云 提 取 ， 
提取 算法 流程 如 下 : 
从 待 处 理 点 云 中 随机 选取 3 个 不 共 线 的 点 ， 得 出 由 这 3 
个 点 确定 的 平面 的 方程 为 
Ax+By+Cz=E (3) 
其 中 4,B,C,E 为 所 确定 平面 方程 的 参数 , 其 他 点 (a,5,c) 到 平面 
的 距离 a; 为 


4 lA atB*brC*e-E| (4) 


VB TC 
di 平均 距离 a 以 及 di 的 标准 偏差 0 为 
了 = 二 》 ud (5) 


na 


Sa-a) (6) 


i=] 


GCT 一 
n—l 


如 图 2 所 示 , 取 距 离 闷 值 :=2o , 当 4 >t 时 , 此 点 被 认为 
是 外 点 ， 予 以 删除 ， 反 之 ， 则 为 内 点 ， 予 以 保留 ， 
内 点 的 总 数 P;， 找 出 内 点 数量 最 多 的 平面 点 集 ， 并 返回 相应 


其 中 ， zp, 是 令 域 的 重心 ， 32 和 ww 分别 表 示 特 征 值 及 特征 向 
量 ， 若 特征 值 满 足 js?2<s42 ， 则 加 即 点 pi; 的 法 向 量 n 。 
2.3.2 法 向 量 定向 
考虑 到 估计 得 到 的 法 向 量 元 方向 具有 不 确定 性 ， 并 没 
确定 法 向 量 的 矢量 方向 ， 故 采用 视线 方向 进行 统一 以 保证 ; 
向 量 方向 的 一 致 性 ， 已 知 视 点 为 w ， 对 所 有 法 向 量 定 向 ， 
它们 朝向 视点 方向 ， 需 满足 如 下 公式 : 

(VP)>0 (14) 


党 泣 : 


3 ” 道 面 坡度 估计 


稀 琉 子 空间 的 平面 优化 方法 ， 可 有 效 减少 异常 值 对 坡度 
估计 的 影响 ， 但 其 最 优 平面 采用 随机 搜索 获取 ， 极 有 可 能 得 
到 局 部 最 优 解 ， 增 加 平面 迭代 次 数 等 问题 。 为 了 减少 随机 取 
值 带 来 的 影响 ， 本 章 在 余弦 k-means 聚 类 中 引入 余弦 距离 ， 
解决 聚 类 初始 点 随机 取 值 问题 。 表 1 列 出 了 本 章 中 用 到 的 重 

表 1 公式 变量 定义 


内 点 的 序号 ， 将 其 记 为 一 个 平面 区 域 。 从 场景 点 云 中 剔除 已 
经 得 到 的 内 点 ， 重 复 上 述 步 又， 直到 场景 中 剩余 的 点 数 小 于 


预先 设 定 的 阔 值 ， 或 者 最 后 一 次 得 到 的 内 点 数量 小 于 阔 值 
[23,24] 。 


图 2 坡 道 阔 值 示 意 棋 
Fig.2 Schematic diagram of ramp threshold 

为 便于 后 续 计 算 ， 对 于 分 割 后 的 各 平面 点 云 中 数据 点 进行 

判断 ， 当 数据 点 超过 10000 时 ,使 用 直通 滤波 来 滤 除 部 分 点 云 。 


CC 


统计 局 Tab. 1 Formula variable definition 

公式 变量 定义 

a,b 单个 平面 任意 两 法 向 量 
dist(a,Bd) 过 与 万 的 欧 氏 距离 
cos(a,b) a 与 pb 的 余弦 相似 度 

人 与 5 的 标准 化 向 量 
dist: (a,b) 4 与 5 的 余弦 距离 

cd 欧式 空间 中 任意 两 个 标准 化 向 量 

3.1 余弦 聚 类 


在 单个 平面 提取 所 得 的 法 向 量 中 , 任意 取 两 个 向 量 a6 在 
欧式 空间 中 表示 为 a=(@,a,…,a,) » pb 
距离 dist(a,D) 为 
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“与 的 余弦 夹 角 定 义 为 余弦 相似 度 ， 在 法 向 量 定向 章 
节 已 保证 向 量 朝向 一 致 ， 故 此 时 cos(a,) 取 值 分 布 在 [0,1] 。 


大 二 Dab > ax 
cos(a,b) EO- (16) 
a a llalHipll 
Da ly be 
k=1 k=] 
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diste(c,d)=1—cos(c,d) (24) 
diste (c,d) =3disr (ed) (25) 


余弦 距离 的 引入 ， 弥 补 了 余弦 k-means 聚 类 缺少 距离 度 
量 的 缺点 。 同 时 作为 纽带 将 基于 欧 氏 距离 的 k-means 算法 迁 
移 到 本 文 余 弦 聚 类 算法 之 中 ， 通 过 式 (25) 可 知 余弦 距离 在 刻 


余弦 相似 度 是 利用 向 量 夹 角 的 余弦 值 来 刻画 相似 性 ， 注 


画 距离 的 方法 上 与 欧 氏 距离 一 致 ,并 且 与 欧 氏 距离 呈正 相关 ， 


重 维 度 间 相对 层面 的 差异 R33， 继续 对 上 述 公 式 推导 。 首 先 对 
欧式 空间 任意 向 量 4=(@,6.…,4,) ，5=(b,b…,b,) 进行 标准 化 : 


js 本 17 

二 [各 备 … 凋 ) (17) 

-入 , 信 … 后 (18) 
BB I 


对 此 余弦 距离 可 表达 与 欧式 距离 一 样 的 距离 意义 ; 同时 余弦 
距离 完全 由 余弦 相似 度 决定 ， 育 类 效果 与 余弦 相似 度 一 致 。 
开 以 将 样本 点 之 间 欧 式 距 离 远 ， 作 为 聚 类 初始 点 的 思想 应 用 
于 余弦 k-means 聚 类 ， 依 次 寻找 余 艾 距离 相对 远 的 K 个 点 。 
该 方法 帮助 余弦 k-means 聚 类 有 条 件 的 选取 初始 聚 类 中 心 ， 
以 减少 初始 聚 类 中 心 随机 取 值 的 不 利 影响 。 设 计 聚 类 流程 如 
图 3 所 示 。 


将 标准 化 向 量 z5， 带 入 式 (2) 得 ;与 无 的 余弦 相似 度 


6 人 万 = Da b; -ba =cos(aD) (19) 
即 向 量 a,8, 在 标准 化 前 后 的 余弦 相似 度 保 持 不 变 ， 此 时 


度 为 1， 式 (16) 可 简化 为 ap, ， 计 算 效率 得 到 有 效 提 


Fely 
地 
森 


电 


聚 类 初始 点 的 选取 是 基于 欧式 距离 的 k-means 算法 研究 
的 热点 问题 P9。 其 核心 是 在 第 一 次 迭代 选 点 时 对 类 别 进 行 预 
测 ， 使 选 点 更 接近 于 理想 值 ， 以 减少 聚 类 过 程 的 迭代 次 数 和 
计算 开销 。 初 始点 的 选取 遵循 以 下 原则 : 距离 大 的 样本 分 到 
同一 类 的 可 能 性 小 ， 相反 ， 距 离 小 的 样本 分 到 同一 类 的 可 能 
性 大 2 。 因 此 ， 聚 类 的 初始 迭代 需要 选取 K 个 相互 之 间距 离 
较 远 的 样本 以 取得 较 好 的 效果 。 如 何在 余弦 聚 类 中 对 空间 向 
量 进 行 第 一 轮 选 点 成 为 接 下 来 需要 解决 的 问题 。 如 果 采 用 余 
弦 相 似 度 小 作为 初始 选 点 的 方式 , 向 量 ,a; 作为 初始 的 两 点 ， 
距离 两 点 尽 可 能 远 的 & 是 极 难 找到 的 P8。 因 为 在 欧 氏 距离 中 ， 
其 第 三 点 & 选取 采用 分 别 到 a,a 的 距离 和 或 积 的 最 大 值 ， 然 
而 和 或 积 在 余弦 中 是 没有 意义 的 , 无 法 表示 相距 较 远 ,因此 引 
入 余弦 距离 diste(a,B) 


[ey 


dist(a,b)=1—cos(a,b) (20) 

于 cos(a,B) 取 值 分 布 在 [0 ， 故 disr(a,B) 取 值 分 布 在 0 到 

1, 且 余 弦 距 离 与 向 量 在 空间 上 夹 角 大 小 呈正 相关 , 即 两 向 量 

在 空间 上 夹 角 越 小 ， 余 弦 距离 数值 越 小 ， 两 向 量 越 容易 分 为 

一 类 ， 故 余弦 距离 可 作为 衡量 距离 的 指标 。 
取 欧 式 空间 中 任意 两 个 标准 化 向 量 “=(oe…'c)， 

d=(di,d…,d,) 继续 推导 欧 氏 距离 disi(e,a4) ， 余 弦 相 似 度 cosc.0) ， 


| 结束 | 


图 3 聚 类 流程 
Clustering flowchart 


Fig. 3 
3.2 ”累加 拟 合 
经 余弦 聚 类 后 的 斜面 和 水 平 再 


[29], 


法 向 量 夹 角 可 定义 为 


(26) 


余弦 距离 disr(c,d) 三 者 之 间 的 关系 。 将 标准 化 向 量 


上 = 国有 = =1， 结 合式 (16) 得 


大 


cd Yod, 
cos(c,d) =——! = cd (21) 
ala HR 
式 (15) 得 


dist2(c,d) = Ye -d= Se —2cd;+d?)=2— 2 cd (22) 
i=1 1 i=1 


i= 


结合 式 (21)， 得 关系 式 cos(6,4d) =1- 了 disr(6,d) ， 可 知 余弦 相 


似 度 与 欧式 距离 具有 直接 函数 关系 ， 两 者 间 存 在 紧密 联系 。 
将 式 (20) 代 入 得 到 欧 氏 距离 dist(e,a) , 余弦 相似 度 cos(c,d) , 余弦 
距离 disre(e,q) 三 者 的 关系 如 下 : 


cos(c,d) =1- 7disr (Cd) (23) 


其 中 : 元 表示 斜坡 面 任 一 点 pi 的 法 线 ; 名 表示 地 面 任 一 点 Pp 
的 法 线 。 

采用 随机 取 值 方法 的 坡度 估计 结果 常 具有 较 大 的 不 确定 
性 ， 故 采用 累加 拟 合 的 方法 完成 坡度 估计 ， 夹 角 值 取 ， 其 
公式 如 下 : 


B= (27) 


4 ”实验 与 分 析 


4.1 实验 平台 

本 文 实验 所 用 移动 机 器 人 结构 如 图 4 所 示 ， 其 底盘 采用 
两 主动 轮 、 两 驱动 轮 的 四 轮 结构 ， 主 动 轮 控制 方向 调整 ， 驱 
动 轮 提供 行进 动力 ， 从 而 完成 移动 机 器 人 的 运动 过 程 。 机 器 
人 配备 了 基于 二 代 Kinect 的 RGB-D 视觉 相机 ， 用 以 感知 环 
境 信息 ， 并 搭载 Core I7 主机 来 完成 数据 处 理 和 运动 控制 。 
4.2 与 常用 方法 比较 

为 验证 本 文 提 出 的 融合 型 算法 的 优越 性 ， 将 其 与 基于 传 
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统 最 小 二 乘法 的 坡度 估计 方法 ， 以 及 基于 稀疏 子 空间 法 的 坡 
度 估算 方法 进行 对 比 。 在 华盛顿 大 学 公开 数据 集 Rgb-scenes- 
V2 中 选取 了 数据 集中 角度 为 900” 的 墙 体 与 地 面 (模拟 道 面 坡 
度 为 900”) 用 于 仿真 实验 分 析 ， 数 据 集 点 云图 如 图 5 所 示 。 


图 4 ”实验 平台 机 器 人 结构 


Fig.4 Experimental platform robot structure 
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差 在 1* 以 上 ， 且 具有 较 多 异常 值 ， 当 上 大 于 100 时 ， 所 得 角 
度 其 误差 变化 稳定 在 0.5" 左 右 ， 综 合 考虑 选择 上 =100 作为 邻 
域 值 。 根 据 拟 合 邻 域 值 上 分 别提 取 和 斜面 和 水 平面 法 向 量 ， 如 
图 8、9 所 示 。 


图 8 斜面 法 向 量 提取 


Fig.8 Normal vector extraction of inclined plane 


图 5 数据 集 点 云图 
Fig.5 Data set point cloud image 
本 文 所 述 融 合 型 算法 实验 步骤 如 下 : 
对 滤波 处 理 后 的 数据 集 进行 平面 分 割 ， 
割 后 得 到 的 斜面 ( 蓝 色 ) 和 水 平面 (红色 )。 


图 6 为 数据 集 分 


> 7 7 


六 
2 
图 6 平面 分 割 结果 

Fig.6 Plane segmentation result 
对 向 量 提取 的 拟 合 邻 域 值 * 取 值 进行 讨论 ， 当 拟 合 邻 域 
范围 的 上 足够 大 时 ， 得 到 的 法 向 量 效果 更 好 ， 噪 声 小 ， 但 随 
之 而 来 的 是 成 倍增 长 的 运算 时 间 和 计算 压力 ， 为 了 保证 运算 


的 实时 性 和 准确 性 。 对 数据 集 点 云 不 同 邻 域 范围 的 上 值 进行 
实验 验证 : 对 于 坡度 为 900” 的 两 平面 ， 分 别 取 * 为 25，35， 


50，60，100，1000，2000 进行 实验 。 图 7 为 不 同上 值 时 得 到 
的 坡 道 夹 角 情 况 ， 随 着 上 值 增 大 ， 夹 角 变 化 趋 于 稳定 ， 测 量 
值 更 接近 于 真实 值 。 


T T T T T T T T 
25 35 50 60 100 500 1000 2000 


邻 域 k 值 
图 7 坡 道 夹 角 随 k 值 变化 情况 
Fig.7 Angle change with k value 
实验 结果 表明 ， 当 小 于 100 时 ， 所 得 角度 与 真实 值 误 


图 9 水 平面 法 向 量 提取 
Fig.9 Normal vector extraction of horizontal plane 

对 单个 平面 的 向 量 进行 余弦 聚 类 后 采用 累加 拟 合 ， 累 加 
拟 合 次 数 在 与 随机 取 值 对 比 后 ， 选 择 设 定 为 100 次 。 部 分 结 
果 如 表 2 所 示 , 全 部 数值 对 比 情况 如 图 10 所 示 , 可 知 累加 拟 
合 方法 在 取 值 100 次 后 趋 于 稳定 ， 而 随机 取 值 方法 计算 结果 
波动 仍 较 大 ， 再 次 证 明 引 入 累加 拟 合 后 ， 极 大 保证 了 融合 型 
算法 的 估计 精度 。 

表 2 累加 拟 合 与 随机 取 值 部 分 对 比 


Tab.2 Comparison between cumulative fitting and random value 


计算 次 数 随机 取 值 累加 拟 合 
1 90.5649 90.8025 
2 89.3503 90.7853 
3 90.8679 90.7172 
4 90.5569 90.7679 
5 90.6058 90.7852 
98 90.9083 90.5353 
99 90.6522 90.5346 
100 90.5425 90.5460 
101 90.5425 90.5471 
102 90.7795 90.5480 

91.0] | | | 
中 | Nh | | 中 中 | pl 


Nn] i [I pt Dy i lL Wi bl i 
| 外 | | | 中 


| | | | 


角度 值 


89.5 了 | 


89.0 十 T T T T 
-20 0 20 40 60 


80 100 120 10 160 180 
计算 次 数 
图 10 累加 拟 合 与 随机 取 值 对 比 


Fig. 10 Comparison chart of cumulative average and random value 


最 终 本 文 算法 在 数据 集 Rgb-scenes-v2 中 90” 墙 面 坡度 
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种 算法 的 估计 结果 对 比 见 表 3。 图 11、12 分 别 给 出 了 基于 传 
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4.3 ”实际 环境 下 的 坡 道 角 度 识 别 
为 进一步 验证 融合 型 算法 在 室外 环境 的 有 效 性 及 可 行 性 ， 


绽 拓 小 二 飞 法 提取 的 创面 及 水 平面 法 向 量 | 
对 比 结果 可 知 , 在 数据 集中 


聚 类 法 结果 引用 文献 [11] 数 据 。 


基于 稀 疏 子 空间 


相 较 最 小 二 


乘法 及 稀疏 子 空 


间 算 法 ， 本 文 提 


出 的 融合 型 算法 


选取 搭载 ROS 系统 的 RGB-D 移动 机 器 人 在 
进行 半 物 理 仿 真实 验 ， 实 验 环 境 如 图 14。 


户外 坡 道 环 境 下 


估计 精度 更 高 ， 相 对 误差 分 别 降 


氏 了 1.21% 和 2.13%。 


图 11 有 提取 法 向 量 


Fig. 11 a extraction normal vector 


图 12 水 平面 提取 法 向 量 
Horizontal plane extraction normal vector 
表 3 数据 集 测 量 结果 分 析 


Tab.3 Analysis of Data set measurement results 


Fig. 12 


图 14 移动 机 器 人 与 坡 道 相 对 位 置 
Fig. 14 The position of the mobile robot and the slope 
未 处 理 的 坡 道 点 云 如 图 15 所 示 ， 其 点 云 边 缘 存在 可 见 
的 离 群 点 ， 离 群 点 对 于 法 向 量 的 提取 是 十 分 不 利 的 ， 需 要 对 
其 进行 滤波 处 理 。 


图 15 坡 道 点 云图 

Fig. 15 Point cloud map of slope 
本 文 使 用 统计 滤波 方法 对 原点 云 进行 处 理 ， 滤 波 参数 设 
置 为 : m=20x=2 。 滤 波 后 点 云 如 图 16， 被 过 滤 的 点 云 离 群 


测量 方法 测量 结果 测量 相对 误差 /* 测量 相对 误差 /% 
最 小 二 乘法 88.36 1.64 1.82 
稀 玖 子 空间 87.53 2.47 2.74 
本 文 方法 90.55 0.55 0.61 


此 外 ， 如 图 13 在 实际 环境 中 ， 搭 建 坡度 分 别 为 16”、 


点 如 图 17， 可 见 点 云 离 群 点 


已 被 有 效 滤 除 。 


47”、60” 的 斜坡 平 玫 


1|， 对 比 最 小 二 


乘法 与 本 文 方法 在 实际 


应 用 场景 下 不 同 坡度 的 


体 表现 。 测 量 结果 见 表 4， 相 较 最 


小 二 乘 算法 ， 本 文 方法 坡度 估计 准确 率 仍 保持 在 较 高 水 准 ， 
， 较 最 小 二 


在 所 给 的 三 种 坡度 下 平均 误差 为 0.86° 
平均 误差 降低 1.43° 


乘 算法 的 


a 上 

图 13 ”室内 搭建 坡度 环境 

Fig. 13 Building a sloped environment indoors 
表 4 实际 环境 测量 结果 分 析 


Tab.4 Analysis of actual environmental measurement results 


测量 方法 “斜坡 角度 测量 结果 测量 相对 误差 /” ”平均 误差 /* 
16 18.63 2.63 

最 小 二 乘法 47 49.27 227 2.29 
60 61.97 1.97 
16 16.89 0.89 

本 文 方法 47 47.75 0.75 0.86 
60 60.93 0.93 


图 16 


滤波 后 斜坡 点 云 
Fig. 16 Slope point cloud after filtering 


图 17 斜坡 点 云 外 围 离 群 点 

Fig. 17 Outliers on the periphery of the slope point cloud 

经 滤波 处 理 后 的 点 云 ， 其 数据 点 较 多 ， 需 要 在 分 割 的 同 
时 进行 降 采 样 处 理 ， 以 满足 点 云 设 定 要 求 。 分 割 后 的 点 云 如 
图 18 所 示 ， 其 中 水 平面 呈 红 色 ， 坡 道 呈 蓝 色 ， 绿 框 标注 部 分 
为 该 坡 道 一 处 台阶 凸 起 , 该 凸 起 对 于 法 向 量 的 生成 存在 干扰 ; 
经 平面 分 制 ,水 平面 (红色 ) 和 坡 道 ( 蓝 色 ) 均 残存 部 分 凸 起 ， 且 
该 坡 道 点 云 点 数 为 12405， 远 超过 设 定 冰 值 ， 故 使 用 直通 滤 
波 法 滤 除 绿 框 所 示 的 凸 起 道 面 点 云 ， 直 至 所 含 点 云 个 数 低 于 
10000 。 
平面 分 割 后 进行 法 向 量 提取 ， 此 时 法 向 量 提取 闵 值 * 设 
定 为 100， 斜 面 和 水 平面 法 向 量 提取 结果 如 图 19、20 所 示 。 
其 中 共 提 取 和 斜面 法 向 量 4329 组 ， 水 平面 法 向 量 4830 组 ; 通 
过 余弦 聚 类 对 异常 值 进 行 聚 类 剔除 ， 之 后 采用 累加 拟 合 ， 拟 
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合 次 数 设 置 为 100， 


最 终 得 到 该 斜坡 角度 为 12.5 度 。 


图 18 披 记 点 去 面 分 制 


图 19 ”斜面 法 向 量 提 取 


Fig. 19 ”Normal vector extraction of inclined plane 


Ul | 


N 


图 20 ”水 平面 法 向 量 提取 


Fig. 20 Normal vector extraction of horizontal plane 
5 ”结束 语 


本 文 提出 一 种 面向 实际 应 用 的 视觉 RGB-D 机 器 人 道路 
坡度 估计 融合 型 算法 。 针 对 坡度 估计 算法 普遍 局 限于 仿真 ， 
在 真实 环境 中 估计 精度 不 高 的 问题 ， 设 计 并 实现 了 融合 统计 
滤波 、 平 面 分 割 、 法 向 量 提 取 、 余 弦 聚 类 和 累加 平均 等 多 种 
方法 的 可 用 于 真实 环境 的 坡 道 估计 方法 ， 并 与 传统 方法 比较 
分 析 。 结 果 表明 所 提出 的 融合 型 算法 在 降低 估计 误差 、 提 高 
坡度 估计 精度 上 有 显著 优势 。 在 数据 集 下 ， 相 较 传统 最 小 二 
乘法 与 稀疏 子 空间 算法 ， 基 于 新 融合 算法 的 坡 道 估 计 误 差分 
别 降 低 1.21%、2.13%; 在 真实 环境 下 ， 本 文 方法 较 最 小 二 乘 
方法 平均 误差 降低 1.43”。 为 其 后 续 移 动机 器 人 在 真实 环境 
中 的 路 径 规划 和 移动 避 障 提供 了 精确 环境 信息 保障 。 
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